///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
*	Contains code for 3D vectors.
*	\file		LaborPoint.h
*	\author		Pierre Terdiman
*	\date		April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __LaborPoint_H__
#define __LaborPoint_H__

#include "LaborTypes.h"
#include "LaborQuat.h"
#include "LaborAxes.h"
#include "LaborFPU.h"
#include "LaborRandom.h"
#include "LaborCoreDef.h"
NAMESPACE_LABOR3D_BEGIN
// Forward declarations
class CLHPoint;
class CLPlane;
class CLMatrix3x3;
class CLMatrix4x4;

#define CROSS2D(a, b)	(a.x*b.y - b.x*a.y)

const float EPSILON2 = 1.0e-20f;

class  LABORCORE_API CLPoint
{
public:
	static const CLPoint ZERO;
	static const CLPoint AXIS_X;
	static const CLPoint AXIS_Y;
	static const CLPoint AXIS_Z;
	static const CLPoint NEGATIVE_AXIS_Z;

public:

	//! Empty constructor
	inline					CLPoint()													{}
	//! Constructor from a single float
	//		inline					CLPoint(float val) : x(val), y(val), z(val)					{}
	// Removed since it introduced the nasty "Point T = *Matrix4x4.GetTrans();" bug.......
	//! Constructor from floats
	inline					CLPoint(float _x, float _y, float _z) : x(_x), y(_y), z(_z)	{}
	//! Constructor from array
	inline					CLPoint(const float f[3]) : x(f[_X]), y(f[_Y]), z(f[_Z])		{}
	//! Copy constructor
	inline					CLPoint(const CLPoint& p) : x(p.x), y(p.y), z(p.z)				{}
	//! Destructor
	inline					~CLPoint()													{}

	//! Clears the vector
	inline	CLPoint&			zero()									{ x =			y =			z = 0.0f;			return *this;	}

	//! + infinity
	inline	CLPoint&			setPlusInfinity()						{ x =			y =			z = MAX_FLOAT;		return *this;	}
	//! - infinity
	inline	CLPoint&			setMinusInfinity()						{ x =			y =			z = MIN_FLOAT;		return *this;	}

	//! Sets positive unit random vector
	CLPoint&			positiveUnitRandomVector();
	//! Sets unit random vector
	CLPoint&			unitRandomVector();

	//! Assignment from values
	inline	CLPoint&			set(float _x, float _y, float _z)		{ x  = _x;		y  = _y;	z  = _z;			return *this;	}
	//! Assignment from array
	inline	CLPoint&			set(const float f[3])					{ x  = f[_X];	y  = f[_Y];	z  = f[_Z];			return *this;	}
	//! Assignment from another CLPoint
	inline	CLPoint&			set(const CLPoint& src)					{ x  = src.x;	y  = src.y;	z  = src.z;			return *this;	}

	//! Adds a vector
	inline	CLPoint&			add(const CLPoint& p)						{ x += p.x;		y += p.y;	z += p.z;			return *this;	}
	//! Adds a vector
	inline	CLPoint&			add(float _x, float _y, float _z)		{ x += _x;		y += _y;	z += _z;			return *this;	}
	//! Adds a vector
	inline	CLPoint&			add(const float f[3])					{ x += f[_X];	y += f[_Y];	z += f[_Z];			return *this;	}
	//! Adds vectors
	inline	CLPoint&			add(const CLPoint& p, const CLPoint& q)		{ x = p.x+q.x;	y = p.y+q.y;	z = p.z+q.z;	return *this;	}

	//! Subtracts a vector
	inline	CLPoint&			sub(const CLPoint& p)						{ x -= p.x;		y -= p.y;	z -= p.z;			return *this;	}
	//! Subtracts a vector
	inline	CLPoint&			sub(float _x, float _y, float _z)		{ x -= _x;		y -= _y;	z -= _z;			return *this;	}
	//! Subtracts a vector
	inline	CLPoint&			sub(const float f[3])					{ x -= f[_X];	y -= f[_Y];	z -= f[_Z];			return *this;	}
	//! Subtracts vectors
	inline	CLPoint&			sub(const CLPoint& p, const CLPoint& q)		{ x = p.x-q.x;	y = p.y-q.y;	z = p.z-q.z;	return *this;	}

	//! this = -this
	inline	CLPoint&			neg()									{ x = -x;		y = -y;			z = -z;			return *this;	}
	//! this = -a
	inline	CLPoint&			neg(const CLPoint& a)						{ x = -a.x;		y = -a.y;		z = -a.z;		return *this;	}

	//! Multiplies by a scalar
	inline	CLPoint&			mult(float s)							{ x *= s;		y *= s;		z *= s;				return *this;	}

	//! this = a * scalar
	inline	CLPoint&			mult(const CLPoint& a, float scalar)
	{
		x = a.x * scalar;
		y = a.y * scalar;
		z = a.z * scalar;
		return *this;
	}

	//! this = a + b * scalar
	inline	CLPoint&			mac(const CLPoint& a, const CLPoint& b, float scalar)
	{
		x = a.x + b.x * scalar;
		y = a.y + b.y * scalar;
		z = a.z + b.z * scalar;
		return *this;
	}

	//! this = this + a * scalar
	inline	CLPoint&			mac(const CLPoint& a, float scalar)
	{
		x += a.x * scalar;
		y += a.y * scalar;
		z += a.z * scalar;
		return *this;
	}

	//! this = a - b * scalar
	inline	CLPoint&			msc(const CLPoint& a, const CLPoint& b, float scalar)
	{
		x = a.x - b.x * scalar;
		y = a.y - b.y * scalar;
		z = a.z - b.z * scalar;
		return *this;
	}

	//! this = this - a * scalar
	inline	CLPoint&			msc(const CLPoint& a, float scalar)
	{
		x -= a.x * scalar;
		y -= a.y * scalar;
		z -= a.z * scalar;
		return *this;
	}

	//! this = a + b * scalarb + c * scalarc
	inline	CLPoint&			mac2(const CLPoint& a, const CLPoint& b, float scalarb, const CLPoint& c, float scalarc)
	{
		x = a.x + b.x * scalarb + c.x * scalarc;
		y = a.y + b.y * scalarb + c.y * scalarc;
		z = a.z + b.z * scalarb + c.z * scalarc;
		return *this;
	}

	//! this = a - b * scalarb - c * scalarc
	inline	CLPoint&			msc2(const CLPoint& a, const CLPoint& b, float scalarb, const CLPoint& c, float scalarc)
	{
		x = a.x - b.x * scalarb - c.x * scalarc;
		y = a.y - b.y * scalarb - c.y * scalarc;
		z = a.z - b.z * scalarb - c.z * scalarc;
		return *this;
	}

	//! this = mat * a
	inline	CLPoint&			mult(const CLMatrix3x3& mat, const CLPoint& a);

	//! this = mat1 * a1 + mat2 * a2
	inline	CLPoint&			mult2(const CLMatrix3x3& mat1, const CLPoint& a1, const CLMatrix3x3& mat2, const CLPoint& a2);

	//! this = this + mat * a
	inline	CLPoint&			mac(const CLMatrix3x3& mat, const CLPoint& a);

	//! this = transpose(mat) * a
	inline	CLPoint&			transMult(const CLMatrix3x3& mat, const CLPoint& a);

	//! Linear interpolate between two vectors: this = a + t * (b - a)
	inline	CLPoint&			lerp(const CLPoint& a, const CLPoint& b, float t)
	{
		x = a.x + t * (b.x - a.x);
		y = a.y + t * (b.y - a.y);
		z = a.z + t * (b.z - a.z);
		return *this;
	}

	//! Hermite interpolate between p1 and p2. p0 and p3 are used for finding gradient at p1 and p2.
	//! this =	p0 * (2t^2 - t^3 - t)/2
	//!			+ p1 * (3t^3 - 5t^2 + 2)/2
	//!			+ p2 * (4t^2 - 3t^3 + t)/2
	//!			+ p3 * (t^3 - t^2)/2
	inline	CLPoint&			herp(const CLPoint& p0, const CLPoint& p1, const CLPoint& p2, const CLPoint& p3, float t)
	{
		float t2 = t * t;
		float t3 = t2 * t;
		float kp0 = (2.0f * t2 - t3 - t) * 0.5f;
		float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f;
		float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f;
		float kp3 = (t3 - t2) * 0.5f;
		x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3;
		y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3;
		z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3;
		return *this;
	}

	//! this = rotpos * r + linpos
	inline	CLPoint&			transform(const CLPoint& r, const CLMatrix3x3& rotpos, const CLPoint& linpos);

	//! this = trans(rotpos) * (r - linpos)
	inline	CLPoint&			invTransform(const CLPoint& r, const CLMatrix3x3& rotpos, const CLPoint& linpos);

	//! Returns MIN(x, y, z);
	inline	float			Min()				const			{ return MIN(x, MIN(y, z));												}
	//! Returns MAX(x, y, z);
	inline	float			Max()				const			{ return MAX(x, MAX(y, z));												}
	//! Sets each element to be componentwise minimum
	inline	CLPoint&			Min(const CLPoint& p)				{ x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z);	return *this;	}
	//! Sets each element to be componentwise maximum
	inline	CLPoint&			Max(const CLPoint& p)				{ x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z);	return *this;	}

	//! Clamps each element
	inline	CLPoint&			clamp(float min, float max)
	{
		if(x<min)	x=min;	if(x>max)	x=max;
		if(y<min)	y=min;	if(y>max)	y=max;
		if(z<min)	z=min;	if(z>max)	z=max;
		return *this;
	}

	//! Computes square magnitude
	inline	float			squareMagnitude()	const		{ return x*x + y*y + z*z;												}
	//! Computes magnitude
	inline	float			magnitude()			const		{ return sqrtf(x*x + y*y + z*z);										}
	//! Computes volume
	inline	float			volume()			const		{ return x * y * z;														}

	//! Checks the CLPoint is near zero
	inline	bool			approxZero()		const		{ return squareMagnitude() < EPSILON2;									}

	//! Tests for exact zero vector
	inline	bool			isZero()			const
	{
		if(IR(x) || IR(y) || IR(z))	
			return false;
		return true;
	}

	//! Checks CLPoint validity
	inline	bool			isValid()			const
	{
		if(!isValidFloat(x))	return false;
		if(!isValidFloat(y))	return false;
		if(!isValidFloat(z))	return false;
		return true;
	}

	//! Slighty moves the CLPoint
	void			tweak(unsigned int coord_mask, unsigned int tweak_mask)
	{
		if(coord_mask&1)	{ unsigned int Dummy = IR(x);	Dummy^=tweak_mask;	x = FR(Dummy); }
		if(coord_mask&2)	{ unsigned int Dummy = IR(y);	Dummy^=tweak_mask;	y = FR(Dummy); }
		if(coord_mask&4)	{ unsigned int Dummy = IR(z);	Dummy^=tweak_mask;	z = FR(Dummy); }
	}

#define TWEAKMASK		0x3fffff
#define TWEAKNOTMASK	~TWEAKMASK
	//! Slighty moves the CLPoint out
	inline	void			tweakBigger()
	{
		unsigned int	Dummy = (IR(x)&TWEAKNOTMASK);	if(!IS_NEGATIVE_FLOAT(x))	Dummy+=TWEAKMASK+1;	x = FR(Dummy);
		Dummy = (IR(y)&TWEAKNOTMASK);	if(!IS_NEGATIVE_FLOAT(y))	Dummy+=TWEAKMASK+1;	y = FR(Dummy);
		Dummy = (IR(z)&TWEAKNOTMASK);	if(!IS_NEGATIVE_FLOAT(z))	Dummy+=TWEAKMASK+1;	z = FR(Dummy);
	}

	//! Slighty moves the CLPoint in
	inline	void			tweakSmaller()
	{
		unsigned int	Dummy = (IR(x)&TWEAKNOTMASK);	if(IS_NEGATIVE_FLOAT(x))	Dummy+=TWEAKMASK+1;	x = FR(Dummy);
		Dummy = (IR(y)&TWEAKNOTMASK);	if(IS_NEGATIVE_FLOAT(y))	Dummy+=TWEAKMASK+1;	y = FR(Dummy);
		Dummy = (IR(z)&TWEAKNOTMASK);	if(IS_NEGATIVE_FLOAT(z))	Dummy+=TWEAKMASK+1;	z = FR(Dummy);
	}

	//! Normalizes the vector
	inline	CLPoint&			normalize()
	{
		float M = x*x + y*y + z*z;
		if(M)
		{
			M = 1.0f / sqrtf(M);
			x *= M;
			y *= M;
			z *= M;
		}
		return *this;
	}

	//! Sets vector length
	inline	CLPoint&			setLength(float length)
	{
		float NewLength = length / magnitude();
		x *= NewLength;
		y *= NewLength;
		z *= NewLength;
		return *this;
	}
	//! Gets vector length add by maohm 2005.1.12 13:43
	inline	float			getLength()                         const
	{
		return sqrtf( x*x + y*y + z*z );
	}	

	//! Clamps vector length
	inline	CLPoint&			clampLength(float limit_length)
	{
		if(limit_length>=0.0f)	// Magnitude must be positive
		{
			float CurrentSquareLength = squareMagnitude();

			if(CurrentSquareLength > limit_length * limit_length)
			{
				float Coeff = limit_length / sqrtf(CurrentSquareLength);
				x *= Coeff;
				y *= Coeff;
				z *= Coeff;
			}
		}
		return *this;
	}

	//! Computes distance to another CLPoint
	inline	float			distance(const CLPoint& b)			const
	{
		return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
	}

	//! Computes square distance to another CLPoint
	inline	float			squareDistance(const CLPoint& b)		const
	{
		return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
	}

	//! Dot product dp = this|a
	inline	float			dot(const CLPoint& p)					const		{	return p.x * x + p.y * y + p.z * z;				}

	///< useful for calculating the angle between the normal of the plane, and another normal
	float			dotPlaneNormal( CLPlane &plane );
	///< useful for determining the plane's relationship with a coordinate in 3-D space
	float			dotPlaneCoord( CLPlane &plane );
	//! Cross product this = a x b
	inline	CLPoint			cross(const CLPoint& b ) const
	{
		CLPoint vResult;

		vResult.x = y * b.z - z * b.y;
		vResult.y = z * b.x - x * b.z;
		vResult.z = x * b.y - y * b.x;

		return vResult;
	}

	//! Vector code ( bitmask = sign(z) | sign(y) | sign(x) )
	inline	unsigned int			vectorCode()						const
	{
		return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29);
	}

	//! Returns largest axis
	inline ELPointComponent	largestAxis()						const
	{
		const float* Vals = &x;
		ELPointComponent m = _X;
		if(Vals[_Y] > Vals[m]) m = _Y;
		if(Vals[_Z] > Vals[m]) m = _Z;
		return m;
	}

	//! Returns closest axis
	inline	ELPointComponent	closestAxis()						const
	{
		const float* Vals = &x;
		ELPointComponent m = _X;
		if(AIR(Vals[_Y]) > AIR(Vals[m])) m = _Y;
		if(AIR(Vals[_Z]) > AIR(Vals[m])) m = _Z;
		return m;
	}

	//! Returns smallest axis
	inline	ELPointComponent	smallestAxis()						const
	{
		const float* Vals = &x;
		ELPointComponent m = _X;
		if(Vals[_Y] < Vals[m]) m = _Y;
		if(Vals[_Z] < Vals[m]) m = _Z;
		return m;
	}

	/** Calculates a reflection vector to the plane with the given normal . 
	@remarks NB assumes 'this' is pointing AWAY FROM the plane, invert if it is not.
	*/
	inline	CLPoint reflect(const CLPoint& normal)
	{
		return CLPoint( *this - ( 2 * this->dot(normal) * normal ) );
	}
	//! Refracts the point
	CLPoint&			refract(const CLPoint& eye, const CLPoint& n, float refractindex, CLPoint& refracted);

	//! Projects the CLPoint onto a plane
	CLPoint&			projectToPlane(const CLPlane& p);

	//! Projects the CLPoint onto the screen
	void			projectToScreen(float halfrenderwidth, float halfrenderheight, const CLMatrix4x4& mat, CLHPoint& projected) const;

	//! Unfolds the CLPoint onto a plane according to edge(a,b)
	CLPoint&			unfold(CLPlane& p, CLPoint& a, CLPoint& b);

	//! Hash function from Ville Miettinen
	inline	unsigned int			getHashValue()						const
	{
		const unsigned int* h = (const unsigned int*)(this);
		unsigned int f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff;	// avoid problems with +-0
		return (f>>22)^(f>>12)^(f);
	}

	//! Stuff magic values in the CLPoint, marking it as explicitely not used.
	void			setNotUsed();
	//! Checks the CLPoint is marked as not used
	bool			isNotUsed()							const;

	// Arithmetic operators

	//! Unary operator for CLPoint Negate = - CLPoint
	inline	CLPoint			operator-()							const		{ return CLPoint(-x, -y, -z);							}

	//! Operator for CLPoint Plus = CLPoint + CLPoint.
	inline	CLPoint			operator+(const CLPoint& p)			const		{ return CLPoint(x + p.x, y + p.y, z + p.z);			}
	//! Operator for CLPoint Minus = CLPoint - CLPoint.
	inline	CLPoint			operator-(const CLPoint& p)			const		{ return CLPoint(x - p.x, y - p.y, z - p.z);			}

	//! Operator for CLPoint Mul   = CLPoint * CLPoint.
	inline	CLPoint			operator*(const CLPoint& p)			const		{ return CLPoint(x * p.x, y * p.y, z * p.z);			}
	//! Operator for CLPoint Scale = CLPoint * float.
	inline	CLPoint			operator*(float s)					const		{ return CLPoint(x * s,   y * s,   z * s );			}
	//! Operator for CLPoint Scale = float * CLPoint.
	inline friend	CLPoint	operator*(float s, const CLPoint& p)				{ return CLPoint(s * p.x, s * p.y, s * p.z);			}

	//! Operator for CLPoint Div   = CLPoint / CLPoint.
	inline	CLPoint			operator/(const CLPoint& p)			const		{ return CLPoint(x / p.x, y / p.y, z / p.z);			}
	//! Operator for CLPoint Scale = CLPoint / float.
	inline	CLPoint			operator/(float s)					const		{ s = 1.0f / s; return CLPoint(x * s, y * s, z * s);	}
	//! Operator for CLPoint Scale = float / CLPoint.
	inline	friend	CLPoint	operator/(float s, const CLPoint& p)				{ return CLPoint(s / p.x, s / p.y, s / p.z);			}

	//! Operator for float DotProd = CLPoint | CLPoint.
	inline	float			operator|(const CLPoint& p)			const		{ return x*p.x + y*p.y + z*p.z;						}
	//! Operator for CLPoint VecProd = CLPoint ^ CLPoint.
	inline	CLPoint			operator^(const CLPoint& p)			const
	{
		return CLPoint(
			y * p.z - z * p.y,
			z * p.x - x * p.z,
			x * p.y - y * p.x );
	}

	//! Operator for Point += Point.
	inline	CLPoint&			operator+=(const CLPoint& p)						{ x += p.x; y += p.y; z += p.z;	return *this;		}
	//! Operator for CLPoint += float.
	inline	CLPoint&			operator+=(float s)								{ x += s;   y += s;   z += s;	return *this;		}

	//! Operator for CLPoint -= CLPoint.
	inline	CLPoint&			operator-=(const CLPoint& p)						{ x -= p.x; y -= p.y; z -= p.z;	return *this;		}
	//! Operator for CLPoint -= float.
	inline	CLPoint&			operator-=(float s)								{ x -= s;   y -= s;   z -= s;	return *this;		}

	//! Operator for CLPoint *= CLPoint.
	inline	CLPoint&			operator*=(const CLPoint& p)						{ x *= p.x; y *= p.y; z *= p.z;	return *this;		}
	//! Operator for Point *= float.
	inline	CLPoint&			operator*=(float s)								{ x *= s; y *= s; z *= s;		return *this;		}

	//! Operator for CLPoint /= CLPoint.
	inline	CLPoint&			operator/=(const CLPoint& p)						{ x /= p.x; y /= p.y; z /= p.z;	return *this;		}
	//! Operator for CLPoint /= float.
	inline	CLPoint&			operator/=(float s)								{ s = 1.0f/s; x *= s; y *= s; z *= s; return *this; }

	// Logical operators

	//! Operator for "if(CLPoint==CLPoint)"
	inline	bool			operator==(const CLPoint& p)			const		{ return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z)));	}
	//! Operator for "if(CLPoint!=CLPoint)"
	inline	bool			operator!=(const CLPoint& p)			const		{ return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z)));	}

	// Arithmetic operators

	//! Operator for CLPoint Mul = CLPoint * CLMatrix3x3.
	inline	CLPoint			operator*(const CLMatrix3x3& mat)		const
	{
		class ShadowMatrix3x3{ public: float m[3][3]; };	// To allow inlining
		const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;

		return CLPoint(
			x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0],
			x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1],
			x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] );
	}

	//! Operator for CLPoint Mul = CLPoint * Matrix4x4.
	inline	CLPoint			operator*(const CLMatrix4x4& mat)		const
	{
		class ShadowMatrix4x4{ public: float m[4][4]; };	// To allow inlining
		const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;

		return CLPoint(
			x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0],
			x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1],
			x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]);
	}

	//! Operator for Point *= CLMatrix3x3.
	inline	CLPoint&			operator*=(const CLMatrix3x3& mat)
	{
		class ShadowMatrix3x3{ public: float m[3][3]; };	// To allow inlining
		const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;

		float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0];
		float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1];
		float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2];

		x = xp;	y = yp;	z = zp;

		return *this;
	}

	//! Operator for Point *= Matrix4x4.
	inline	CLPoint&			operator*=(const CLMatrix4x4& mat)
	{
		class ShadowMatrix4x4{ public: float m[4][4]; };	// To allow inlining
		const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;

		float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0];
		float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1];
		float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2];

		x = xp;	y = yp;	z = zp;

		return *this;
	}

	// Cast operators

	inline		 bool		operator < ( const CLPoint& rhs ) const
	{
		if( x < rhs.x && y < rhs.y && z < rhs.z )
			return true;
		return false;
	}

	inline		 bool		operator > ( const CLPoint& rhs ) const
	{
		if( x > rhs.x && y > rhs.y && z > rhs.z )
			return true;
		return false;
	}



	//! Cast a Point to a CLHPoint. w is set to zero.
	operator	CLHPoint()				const;

	inline					operator	const	float*() const	{ return &x; }
	inline					operator			float*()		{ return &x; }
	inline CLPoint randomDeviant( const float& angle, const CLPoint& up = CLPoint(0.f, 0.f, 0.f)) const
	{
		CLPoint newUp;

		if (up == CLPoint(0.f, 0.f, 0.f))
		{
			// Generate an up vector
			newUp = this->perpendicular();
		}
		else
		{
			newUp = up;
		}

		// Rotate up vector by random amount around this
		CLQuat q;
		//float angleTemp = unitRandomFloat() * 360.f;
		float angleTemp = unitRandomFloat() * 2 * PI;

		q.fromAngleAxis( angleTemp, *this );
		newUp = q * newUp;

		// Finally rotate this by given angle around randomised up
		float angleDegree = angle * PI/180.f;
		q.fromAngleAxis( static_cast<float>(angleDegree), newUp );
		return q * (*this);


		////////////////////////////////// 
		//// rock_fixed

	}
	inline CLPoint perpendicular(void) const
	{
		static const float fSquareZero = (float)(1e-06 * 1e-06);

		CLPoint perp = this->cross( getAxisX() );

		// Check length
		if( perp.squareMagnitude() < fSquareZero )
		{
			/* This vector is the Y axis multiplied by a scalar, so we have 
			to use another axis.
			*/
			perp = this->cross( getAxisY() );
		}
		return perp;
	}

	/** Gets the shortest arc quaternion to rotate this vector to the destination
	vector.
	@remarks
	If you call this with a dest vector that is close to the inverse
	of this vector, we will rotate 180 degrees around the 'fallbackAxis'
	(if specified, or a generated axis if not) since in this case
	ANY axis of rotation is valid.
	*/
	CLQuat getRotationTo(const CLPoint& dest,
		const CLPoint& fallbackAxis = CLPoint::ZERO) const
	{
		// Based on Stan Melax's article in Game Programming Gems
		CLQuat q;
		// Copy, since cannot modify local
		CLPoint v0 = *this;
		CLPoint v1 = dest;
		v0.normalize();
		v1.normalize();

		CLPoint c = v0.cross(v1);

		float d = v0.dot(v1);

		// If dot == 1, vectors are the same
		if (d >= 1.0f)
		{
			return CLQuat::IDENTITY;
		}
		float s = fastSqrt( (1+d)*2 );
		if (s < 1e-6f)
		{
			if (fallbackAxis != CLPoint::ZERO)
			{
				// rotate 180 degrees about the fallback axis
				q.fromAngleAxis( PI, fallbackAxis);
			}
			else
			{
				// Generate an axis
				CLPoint axis = CLPoint::AXIS_X.cross(*this);
				if (axis.isZeroLength()) // pick another if colinear
					axis = CLPoint::AXIS_Y.cross(*this);
				axis.normalize();
				q.fromAngleAxis(PI, axis);
			}
		}
		else
		{
			float invs = 1 / s;

			q.x = c.x * invs;
			q.y = c.y * invs;
			q.z = c.z * invs;
			q.w = s * 0.5f;
		}
		/// rock_fixed
		return q;
	}
	/** Returns true if this vector is zero length. */
	inline bool isZeroLength(void) const
	{
		float sqlen = (x * x) + (y * y) + (z * z);
		return (sqlen < (1e-06 * 1e-06));

	}

public:
	float			x, y, z;
public:
	inline void getAngleFormVector( const CLPoint &vVector, float &fangle ) const
	{
		fangle = acos( dot( vVector ) );
		if( fangle < 1 || fangle > 0)
		{
		}
		else
		{
			fangle = 0;
		}
	}


	inline static	const CLPoint&	getAxisX()      { return AXIS_X; }
	inline static	const CLPoint&	getAxisY()      { return AXIS_Y; }
	inline static	const CLPoint&	getAxisZ()      { return AXIS_Z; }
	inline static	const CLPoint&	getZero()		{ return ZERO; }



};

void normalize1(CLPoint& a);
void normalize2(CLPoint& a);

NAMESPACE_LABOR3D_END

#endif //__LaborPoint_H__
